clear 
%subplot(1,2,1);
obsHDL = plot(0, 0, 'd');
title('Raw Data');
xlabel('Longitude');
ylabel('Latitude');
axis([-6000 6000 0 9000]);
set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal

% subplot(1,2,2);
% filterHDL = plot(0, 0, 'd');
% title('Filter Output');

% axis([-600 600 0 650]);
% set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal

hold on
obsTxt = text(0, 0, sprintf('(%.3f,%.3f)',0,0), ...
    'Color',[0.2 0.2 0.2], 'FontSize',8, ...
    'HorizontalAlignment','left', 'VerticalAlignment','top');

xlabel('Longitude');
ylabel('Latitude');

[B,A]=butter(1,.08,'low');

R = 6371000;
CC = 0.9;
FC = 0;
FCT = 2;
RC = 0;
RCT = 4;

isFirst = 1;
isCali = 1;
corrA = [];

while 1
     obsC = urlread('http://192.168.1.102/obs.txt');
%      fp = fopen('~/autoBoat/log/obs.txt');
%      obsC = fscanf(fp,'%s');
%      fclose(fp);
    
    obsX = [];
    obsY = [];
    obsV = [];
    
     for i = 1: 181
         value = str2double(myParse(obsC, strcat('value', num2str(i-1))));
         obsV = [obsV, value];

     end
           
     if isFirst
         obsVL = obsV;
         isFirst = 0;
     end
     
     if isCali
         corrM = corrcoef(obsV, obsVL);
         if abs(corrM(2)) > CC
             disp('Similar!');
             FC = FC + 1;
             if FC > FCT
                 isCali = 0;
                 obsVR = obsV;
             end
             
         else
             FC = 0;
         end
         obsVL = obsV;
         
     else
         
         % correlation coefficient
         
         corrM = corrcoef(obsV, obsVR);
         if abs(corrM(2)) > CC
             
             % remove individual points
%              
%              counter = 1;
%              
%              for k = 1 : 180
%                  if abs(obsV(k+1) - obsV(k)) > 100
%                      if counter < 10
%                          for m = k-counter+1 : k
%                              if m > 3
%                                  obsV(m) = (obsV(m-3)+obsV(m-2)+obsV(m-1))/3;
%                              else
%                                  obsV(m) = 8200;
%                              end
%                          end
%                          
%                      end
%                      counter = 1;
%                  else
%                      counter = counter + 1;
%                  end
%              end
%              
%              if counter < 10
%                  for m = 182-counter : 181
%                      obsV(m) = (obsV(m-3)+obsV(m-2)+obsV(m-1))/3;
%                  end
%              end
             
             % individual filter end %
             
             disp('refresh...');
             obsVL = obsV;
             obsVR = obsV;
         else
             
             disp('retain...');
             corrM = corrcoef(obsV, obsVL);
             if abs(corrM(2)) > CC
                 RC = RC + 1;
                 if RC > RCT
                     obsVR = obsV;
                 end
             end
             
             obsVL = obsV;
         end
         
         
                  % PARSE SICK DATA %
         for i = 1: 181
             value = obsV(i);
             if value > 8182
                 continue
             else
                 tempI = 90.0 / 180 * i+45;
                 tempI = (tempI*pi)/180;
                 tempX = cos(tempI)*value;
                 tempY = sin(tempI)*value;
                 obsX = [obsX, tempX];
                 obsY = [obsY, tempY];

             end

             
         end
         set(obsHDL, 'XData', obsX, 'YData', obsY);
         
         %     padding = ones(1,20) * obsY(1);
         %     padding = [padding, obsY];
         %     filterY = filter(B,A,padding);
         %     set(filterHDL, 'XData', obsX, 'YData', filterY(21:end));
         drawnow
         pause(.1);
     end
end